#include "Joy.h"
#include "../Subsystems/chassis.h"
#include "../Subsystems/launcher.h"
#include "../Subsystems/turret.h"

TurretJoy::TurretJoy() 
{
	Requires(turret);
	SetTimeout(0.5);
	//printf("dddd::DriveJoy\n");
}
void TurretJoy::Initialize()
{
	
}
void TurretJoy::Execute()
{
	turret->set(CommandBase::oi->getTurretAxis());
}
bool TurretJoy::IsFinished()
{
	return IsTimedOut();
}
void TurretJoy::End()
{
	
}
void TurretJoy::Interrupted()
{
	
}
DriveJoy::DriveJoy() 
{
	Requires(chassis);
	SetTimeout(0.1);
//	printf("DriveJoy::DriveJoy\n");
}
void DriveJoy::Initialize()
{
	
}
void DriveJoy::Execute()
{
	if(chassis->state==chassis->kNormal)
		chassis->setLeftRight(CommandBase::oi->getDriveLeftAxis(),CommandBase::oi->getDriveRightAxis());
	else if(chassis->state==chassis->kSlow)
		chassis->setLeftRight(SLOW_MODE_FACTOR*CommandBase::oi->getDriveLeftAxis(),SLOW_MODE_FACTOR*CommandBase::oi->getDriveRightAxis());
	 
	//else                                                                                                                                                                                                    
		//printf("balancemode \n");

}
bool DriveJoy::IsFinished()
{
	return IsTimedOut();
}
void DriveJoy::End()
{
	
}
void DriveJoy::Interrupted()
{
	
}
LifterJoy::LifterJoy() 
{
	Requires(lifter);
	SetTimeout(0.5);
	//printf("dddd::LiftJoy\n");
}
void LifterJoy::Initialize()
{
	
}
void LifterJoy::Execute()
{
	if(CommandBase::oi->getLifterForwardButton())
		lifter->Forward();
	else if(CommandBase::oi->getLifterReverseButton())
		lifter->Reverse();
	else
		lifter->Stop();
}
bool LifterJoy::IsFinished()
{
	return IsTimedOut();
}
void LifterJoy::End()
{
	
}
void LifterJoy::Interrupted()
{
	
}
LauncherJoy::LauncherJoy() 
{
	Requires(launcher);
	SetTimeout(0.5);
//	printf("dddd::LJoy\n");
}
void LauncherJoy::Initialize()
{
	
}
void LauncherJoy::Execute()
{
	float joyval=CommandBase::oi->getLauncherAxis();
	launcher->fire(joyval,joyval);
}
bool LauncherJoy::IsFinished()
{
	return IsTimedOut();
}
void LauncherJoy::End()
{
	
}
void LauncherJoy::Interrupted()
{
	
}
RollerJoy::RollerJoy() 
{
	Requires(roller);
	SetTimeout(0.5);
	//printf("dddd::RollerJoy\n");
}
void RollerJoy::Initialize()
{
	
}
void RollerJoy::Execute()
{
	if(CommandBase::oi->getRollerForwardButton())
		roller->Forward();
	else if(CommandBase::oi->getRollerReverseButton())
		roller->Reverse();
	else
		roller->Stop();
}
bool RollerJoy::IsFinished()
{
	return IsTimedOut();
}
void RollerJoy::End()
{
	
}
void RollerJoy::Interrupted()
{
	
}

ConveyerJoy::ConveyerJoy() 
{
	Requires(conveyer);
	SetTimeout(0.5);
	//printf("dddd::RollerJoy\n");
}
void ConveyerJoy::Initialize()
{
	
}
void ConveyerJoy::Execute()
{
	if(CommandBase::oi->getConveyerForwardButton())
		conveyer->Forward();
	else if(CommandBase::oi->getConveyerReverseButton())
		conveyer->Reverse();
	else
		conveyer->Stop();
}
bool ConveyerJoy::IsFinished()
{
	return IsTimedOut();
}
void ConveyerJoy::End()
{
	
}
void ConveyerJoy::Interrupted()
{
	
}
